#include <Arduino.h>

#undef max
#undef min

#include <Firmata.h>

using namespace firmata;
#define MINIMUM_SAMPLING_INTERVAL 1
//#define SERIAL_DEBUG
#include "utility/firmataDebug.h"
#include <LT8920.h>
#include"retain_mcu.h"
const char *fwname = "KB9108_ori";
const int build = BUILD_NUMBER;
SPIClass spi;
LT8920 lt8920(spi, PA4, PA3, PA2);
HardwareSerial Serial1(PA10, PA9);
typedef struct {
    uint8_t sync[5];
    bool synced;
} kb9108_t;
kb9108_t retain;
retain_mcu rem;
int mode;
// DigitalOut *led = new DigitalOut(PB_5); //*上拉
#define led PB5
uint8_t Yo_port[] = {
        PB10,
        PB11,
        PB12,
        PB14,
        PB15,
        PA8,
        PA11,
        PA12,
        PA1,
        PA0,
        PF1,
        PF0,
        PC5,
        PC14,
        PC13,
        PB8,
};
#define DO_NRS sizeof(Yo_port)
byte reportPINs[DO_NRS];   // 1 = report this port, 0 = silence
byte previousPINs[DO_NRS]; // previous 8 bits sent
bool isResetting = false;
/* pins configuration */
byte portConfigInputs[DO_NRS]; // each bit: 1 = pin in INPUT, 0 = anything else
String selfId;
unsigned long currentMillis;        // store the current value from millis()
unsigned long previousMillis;       // for comparison with currentMillis
unsigned int samplingInterval = 19; // how often to run the main loop (in ms)

static const int SYSEX_SET_ID = 0xC0; // MIDI Reserved for realtime messages
static const int SYSEX_CLIENT_GET_ID = 0xC1; // MIDI Reserved for realtime messages
static const int SYSEX_CLIENT_REPORT = 0xC2; // MIDI Reserved for realtime messages
static const int SYSEX_CLIENT_NRS = 0xC3; // MIDI Reserved for realtime messages

void outputPort(firmata::FirmataClass firm, byte portNumber, byte portValue, byte forceSend) {
    // pins not configured as INPUT are cleared to zeros
    portValue = portValue & portConfigInputs[portNumber];
    // only send if the value is different than previously sent
    if (forceSend || previousPINs[portNumber] != portValue) {
        firm.sendDigitalPort(portNumber, portValue);
        previousPINs[portNumber] = portValue;
    }
}

void digitalWriteCallback(firmata::FirmataClass &firm, byte port, int value) {
    byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;

    if (port < DO_NRS) {
        // create a mask of the pins on this port that are writable.
        lastPin = port * 8 + 8;
        if (lastPin > DO_NRS)
            lastPin = DO_NRS;
        for (pin = port * 8; pin < lastPin; pin++) {
            // do not disturb non-digital pins (eg, Rx & Tx)
            if (IS_PIN_DIGITAL(pin)) {
                // do not touch pins in PWM, ANALOG, SERVO or other modes
                if (firm.getPinMode(pin) == OUTPUT || firm.getPinMode(pin) == INPUT) {
                    pinValue = ((byte) value & mask) ? 1 : 0;
                    if (firm.getPinMode(pin) == OUTPUT) {
                        pinWriteMask |= mask;
                    } else if (firm.getPinMode(pin) == INPUT && pinValue == 1 && firm.getPinState(pin) != 1) {
                        // only handle INPUT here for backwards compatibility
                        pinMode(pin, INPUT_PULLUP);
                    }
                    firm.setPinState(pin, pinValue);
                }
            }
            mask = mask << 1;
        }
        writePort(port, (byte) value, pinWriteMask);
    }
}

void reportDigitalCallback(firmata::FirmataClass &firm, byte port, int value) {
    if (port < DO_NRS) {
        reportPINs[port] = (byte) value;
        if (value)
            outputPort(firm, port, readPort(port, portConfigInputs[port]), true);
    }
}

void setPinModeCallback(firmata::FirmataClass &firm, byte pin, int mode) {
    if (firm.getPinMode(pin) == PIN_MODE_IGNORE)
        return;

    if (IS_PIN_DIGITAL(pin)) {
        if (mode == INPUT || mode == PIN_MODE_PULLUP) {
            portConfigInputs[pin / 8] |= (1 << (pin & 7));
        } else {
            portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
        }
    }
    firm.setPinState(pin, 0);
    switch (mode) {
        case PIN_MODE_ANALOG:
            if (IS_PIN_ANALOG(pin)) {
                if (IS_PIN_DIGITAL(pin)) {
                    pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
#if ARDUINO <= 100
                    // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
                    digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
#endif
                }
                firm.setPinMode(pin, PIN_MODE_ANALOG);
            }
            break;
        case INPUT:
            if (IS_PIN_DIGITAL(pin)) {
                pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
#if ARDUINO <= 100
                // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
                digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
#endif
                firm.setPinMode(pin, INPUT);
            }
            break;
        case PIN_MODE_PULLUP:
            if (IS_PIN_DIGITAL(pin)) {
                pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
                firm.setPinMode(pin, PIN_MODE_PULLUP);
                firm.setPinState(pin, 1);
            }
            break;
        case OUTPUT:
            if (IS_PIN_DIGITAL(pin)) {
                if (firm.getPinMode(pin) == PIN_MODE_PWM) {
                    // Disable PWM if pin mode was previously set to PWM.
                    digitalWrite(PIN_TO_DIGITAL(pin), LOW);
                }
                pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
                firm.setPinMode(pin, OUTPUT);
            }
            break;

        default:
            firm.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
    }
    // TODO: save status to EEPROM here, if changed
}

void setPinValueCallback(firmata::FirmataClass &firm, byte pin, int value) {
    if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
        if (firm.getPinMode(pin) == OUTPUT) {
            firm.setPinState(pin, value);
            digitalWrite(PIN_TO_DIGITAL(pin), value);
        }
    }
}

void sysexCallback(firmata::FirmataClass &firm, byte command, byte argc, byte *argv) {

    switch (command) {
        case SYSEX_SET_ID:
            lt8920.setSyncWordLength(argc);
            lt8920.setSyncWord(argv);
            for(int i=0;i<argc;i++)
            retain.sync[i]=argv[i];
            retain.synced=true;
            break;
        case SAMPLING_INTERVAL:
            if (argc > 1) {
                samplingInterval = argv[0] + (argv[1] << 7);
                if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
                    samplingInterval = MINIMUM_SAMPLING_INTERVAL;
                }
            } else {
                //Firmata.sendString("Not enough data");
            }
            break;

        case CAPABILITY_QUERY:
            firm.write(START_SYSEX);
            firm.write(CAPABILITY_RESPONSE);
            for (byte pin = 0; pin < TOTAL_PINS; pin++) {
                if (IS_PIN_DIGITAL(pin)) {
                    firm.write((byte) INPUT);
                    firm.write(1);
                    firm.write((byte) PIN_MODE_PULLUP);
                    firm.write(1);
                    firm.write((byte) OUTPUT);
                    firm.write(1);
                }
#ifdef FIRMATA_SERIAL_FEATURE
                serialFeature.handleCapability(pin);
#endif
                firm.write(127);
            }
            firm.write(END_SYSEX);
            break;
        case PIN_STATE_QUERY:
            if (argc > 0) {
                byte pin = argv[0];
                firm.write(START_SYSEX);
                firm.write(PIN_STATE_RESPONSE);
                firm.write(pin);
                if (pin < TOTAL_PINS) {
                    firm.write(firm.getPinMode(pin));
                    firm.write((byte) firm.getPinState(pin) & 0x7F);
                    if (firm.getPinState(pin) & 0xFF80)
                        firm.write((byte)(firm.getPinState(pin) >> 7) & 0x7F);
                    if (firm.getPinState(pin) & 0xC000)
                        firm.write((byte)(firm.getPinState(pin) >> 14) & 0x7F);
                }
                firm.write(END_SYSEX);
            }
            break;
    }
}

void systemResetCallback(firmata::FirmataClass &firm) {
    isResetting = true;

#ifdef FIRMATA_SERIAL_FEATURE
    serialFeature.reset();
#endif

    for (byte i = 0; i < DO_NRS; i++) {
        reportPINs[i] = false;   // by default, reporting off
        portConfigInputs[i] = 0; // until activated
        previousPINs[i] = 0;
    }

    for (byte i = 0; i < TOTAL_PINS; i++) {
        // pins with analog capability default to analog input
        // otherwise, pins default to digital output
        if (IS_PIN_ANALOG(i)) {
            // turns off pullup, configures everything
            setPinModeCallback(firm, i, PIN_MODE_ANALOG);
        } else if (IS_PIN_DIGITAL(i)) {
            // sets the output to 0, configures portConfigInputs
            setPinModeCallback(firm, i, OUTPUT);
        }
    }

    isResetting = false;
}

//void _exit() {}
void setup() {
    uint32_t i;

    // selfId = getUniqueIDAsString();
    Serial1.printf("COM1: sn=%s\n", selfId.c_str());
    rem.begin(sizeof(retain),64*1024-512,64*1024-1024,(const u8*)"KB9108_ori",10,0);
    if(rem.check()){
        rem.remind(0,sizeof(retain),&retain);
    }
    digitalWrite(led, LOW);
    for (i = 0; i < DO_NRS; i++) {
        pinMode(Yo_port[i], OUTPUT);
    }

    Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);

    Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
    Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
    Firmata.attach(SET_PIN_MODE, setPinModeCallback);
    Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
    Firmata.attach(START_SYSEX, sysexCallback);
    Firmata.attach(SYSTEM_RESET, systemResetCallback);
    // lt8920.setRxCb(rxcb);
    lt8920.begin();
    if (lt8920.id1 != 0xf413) {
        printf("lt8920 error!!!");
    }
    // lt8920.setCurrentControl(0xf, 0xf);
    lt8920.setDataRate(LT8920::LT8920_62KBPS);
    lt8920.setChannel(48);
    lt8920.startListening();
    Firmata.begin(lt8920);
    if(retain.synced){
        mode=1;
    }
    else{
        mode=0;
    }
}

int j;

void loop() {

    std::string s;
    // printf("printf");
    j++;
    digitalWrite(led, j & 1);

    switch (mode) {
        case 0: //*发送上线请求 ,芯片id,产品标识
                Serial1.printf("%d:%s\n", s.length(), s.c_str());
                Firmata.write(START_SYSEX);
                Firmata.write(SYSEX_CLIENT_GET_ID);
                Firmata.write(END_SYSEX);
                delay(1000);
            break;
        case 1:
            pinMode(PB_6, INPUT);
            if (!digitalRead(PB_6)) {
                retain.synced=false;
                mode = 0;
            }
            break;
    }
    lt8920.loop();
    rem.retain(0,sizeof(retain),&retain);
}
// int main()
// {
//     setup();
//     while (1)
//         loop();
// }